
#include "FreeRTOS.h"
#include "sensors.h"
#include "FreeRTOSConfig.h"
#include "task.h"
#include "queue.h"
#include "semphr.h"
#include <p32xxxx.h>
#include "main_peripherals.h"
#include "driverI2C_ISR.h"
#include "rtos_queues.h"
#include "state.h"
#include "driverRN41_UART_DMA_ISR_TASK.h"
#include "driverESC_I2C_ISR.h"
#include "quadStability_TASK.h"
#include "config.h"
#include "qmath.h"
#include "pid.h"
#include "driverSD_SPI_DMA_ISR_TASK.h"
#include "remote_TASK.h"
#include "commands.h"

// 116308 before plib remove
// 111944 after plib remove
// 111032 after masking

#pragma config FSRSSEL			= PRIORITY_7
#pragma config FMIIEN			= ON
#pragma config FETHIO			= ON
#pragma config FUSBIDIO			= OFF
#pragma config FVBUSONIO		= OFF

/** 60 MHz internal */
 /*#pragma config FNOSC = FRCPLL // osc select
 #pragma config POSCMOD = OFF // primary osc
 #pragma config FPLLIDIV = DIV_2 // PLL input div
 #pragma config FPLLMUL = MUL_15 // PLL multiplier
 #pragma config FPLLODIV = DIV_1 // PLL output div
 */

/** 76 MHz external - 8mhz crystal */
#pragma config FNOSC			= PRIPLL // osc select
#pragma config POSCMOD			= XT // primary osc
#pragma config FPLLIDIV			= DIV_2 // PLL input div
#pragma config FPLLMUL			= MUL_20 // PLL multiplier
#pragma config FPLLODIV			= DIV_1 // PLL output div
#pragma config UPLLIDIV			= DIV_1	// usb stuff
#pragma config UPLLEN			= OFF		// usb stuff

#pragma config FSOSCEN			= OFF // sec osc enable
#pragma config IESO				= OFF // internal external switch over bit
#pragma config OSCIOFNC			= OFF // clk0 enable bit
#pragma config FPBDIV			= DIV_1 // bootup pbclk divider
#pragma config FCKSM			= CSDCMD // clock switching / monitoring (both disabled)
#pragma config WDTPS			= PS16384	// watchdog postscaler
#pragma config FWDTEN			= OFF // watchdog timer

#pragma config DEBUG			= ON
#pragma config ICESEL			= ICS_PGx2 // i2c/icd comm channel
#pragma config PWP				= OFF // flash write protect
#pragma config BWP				= OFF // boot flash write protect
#pragma config CP				= OFF // code protect

TaskHandle_t _taskMain;
TaskHandle_t _taskBT;
TaskHandle_t _taskStab;
TaskHandle_t _taskSD;
TaskHandle_t _taskRemote;
TaskHandle_t _taskESC;

#define buzzErrorLoadValue 0xFF400000
volatile unsigned int buzzError;

/**
 * Flash all the LEDs & buzzer
 */
void ledBootCycle()
{
	const int delay = configTICK_RATE_HZ / 36;
	
	int i;
	for (i=0;i<4;i++)
	{
		LEDR_ON;
		vTaskDelay(delay);
        BUZZ_OFF;
		LEDG_ON;
		vTaskDelay(delay);
		LEDB_ON;
		vTaskDelay(delay);
		LEDR_OFF;
		vTaskDelay(delay);
		LEDG_OFF;
		vTaskDelay(delay);
		LEDB_OFF;
		vTaskDelay(delay);
	}
}

void q32Test()
{
    q_t a, b, c;

    a = Q_ONE;
    b = -Q_ONE;
    c = qMul(a,b);

    a = -1;
    b = Q_ONE*2;
    c = qMul(a,b);

    a = 0x3FFFFFFF;
    b = Q_ONE*2;
    c = qMul(a,b);

    a = 0xC0000001;
    b = -Q_ONE;
    c = qMul(a,b);

    a = 0xC0000001;
    b = -Q_ONE*2;
    c = qMul(a,b);
}

void atan2test()
{
    q_t a, b;
    q_t c;
    
    for (a = Q_ONE * -4; a <= Q_ONE * 4; a += Q_ONE / 4)
    {
        for (b = Q_ONE * -4; b <= Q_ONE * 4; b += Q_ONE / 4)
        {
            c = q32atan2(a,b);
            c = q32atan(b);
        }
    }
    while(1);
}

void checkFlightMode()
{
    int i;
    for (i=0;i<4;i++)
    {
        BYTE bitMode = i == 0 ? 0 : (1 << (i-1));

        if (CONFIG.base.flightmode[i].range1[0] == CONFIG.base.flightmode[i].range1[1]) continue;
        
        BYTE channel = CONFIG.base.flightmode[i].channel1;
        
        if (STATE.rc.axis[channel] >= CONFIG.base.flightmode[i].range1[0] &&
            STATE.rc.axis[channel] < CONFIG.base.flightmode[i].range1[1])
        {
            channel = CONFIG.base.flightmode[i].channel2;
            if ((CONFIG.base.flightmode[i].range2[0] == CONFIG.base.flightmode[i].range2[1]) ||
                (STATE.rc.axis[channel] >= CONFIG.base.flightmode[i].range2[0] &&
                STATE.rc.axis[channel] < CONFIG.base.flightmode[i].range2[1])
                )
            {
                if (STATE.imu.mode == bitMode) return;
                
                if (i == 0)
                {
                    // disarm
                    struct CommandIn_t disarm;
                    disarm.command = CMD_ESC_SET_MODE_OFF;
                    disarm.paramLength = 0;
                    commandPost(&disarm);
                }
                else if (STATE.imu.mode == 0)
                {
                    if (CONFIG.base.motors != STATE.motors.count) return;
                    // arm!
                    struct CommandIn_t disarm;
                    disarm.command = CMD_ESC_SET_MODE_FLIGHT;
                    disarm.paramLength = 0;
                    commandPost(&disarm);
                }
                
                pidSwitchToMode(bitMode, STATE.imu.mode);

                STATE.imu.mode = bitMode;


                
                return;
            }
        }
    }
}

unsigned int vLipo;
unsigned int current;
unsigned int currentCount = 0;

	TickType_t taskSleep, sensorLED;

void TaskMain(void * parameters)
{
	queuesCreate();
	peripheralsInit();
	configDefault();
	ledBootCycle();
	i2cInit();
    logInit();
	BUZZ(BUZZ_DELAY_NOTIFY,1);
	xTaskCreate(TaskSD,(signed char*)"SD", 260, (void*)NULL, 2, &_taskSD);
    xTaskCreate(TaskRN41,(signed char*)"BT", 260, (void*)NULL, 4, &_taskBT);
    xTaskCreate(TaskRemote,(signed char*)"RE", 260, (void*)NULL, 1, &_taskRemote);

    while (!STATE.system.configLoaded)
        vTaskDelay(configTICK_RATE_HZ/4);

    xTaskCreate(TaskESC,(signed char*)"ES", 260, (void*)NULL, 4, &_taskESC);

    sensorsInit();

    BUZZ(BUZZ_DELAY_NOTIFY,1);

	xTaskCreate(TaskStability,(signed char*)"ST", 670, (void*)NULL, 3, &_taskStab);

	taskSleep = sensorLED = xTaskGetTickCount();
    BUZZ(BUZZ_DELAY_NOTIFY,1);
    int adcCounter = 0;

    for(;;)
	{
		vTaskDelay(1);
        STATE.monitoring.quadTaskTickCount++;
        //if (STATE.monitoring.quadTaskTickCount % 1000 == 0)
        //    BUZZ_ON;
        //else
        //    BUZZ_OFF;

        if (AD1CON1bits.DONE)
        {
            vLipo = ADC1BUF0 + ADC1BUF2 + ADC1BUF4 + ADC1BUF6 + ADC1BUF8 + ADC1BUFA + ADC1BUFC + ADC1BUFE;
            vLipo /= 8;

            if (STATE.monitoring.vLipo == 0)
                STATE.monitoring.vLipo = vLipo;
            else
                STATE.monitoring.vLipo = qMul(STATE.monitoring.vLipo, Q_ONE * 99 / 100) + qMul(vLipo, CONFIG.base.vLipoScale) / 100;

            current = ADC1BUF1 + ADC1BUF3 + ADC1BUF5 + ADC1BUF7 + ADC1BUF9 + ADC1BUFB + ADC1BUFD + ADC1BUFF;
            current /= 8;
            
            STATE.monitoring.current += qMul((current - CONFIG.base.currentOffset), CONFIG.base.currentScale);
            AD1CON1CLR = _AD1CON1_DONE_MASK;
            adcCounter++;
            if (adcCounter >= 1000)
            {
                adcCounter = 0;
                logStore(taskSleep, LOG_ID_POWER_CURRENT, STATE.monitoring.vLipo);

                if (STATE.monitoring.vLipo < CONFIG.base.vLipoCritical)
                {
                    if (STATE.imu.mode == 0) BUZZ(BUZZ_DELAY_NOTIFY,4);
                    else BUZZ_ON;
                }
                else if (STATE.monitoring.vLipo < CONFIG.base.vLipoWarning)
                {
                    if (STATE.imu.mode == 0) BUZZ(BUZZ_DELAY_NOTIFY,1);
                    else BUZZ_TOGGLE;
                }
                else
                {
                    BUZZ_OFF;
                }
            }
        }
        AD1CON1SET = _AD1CON1_ASAM_MASK;


        taskSleep = xTaskGetTickCount();
        if (STATE.hmc5883.connected &&
            STATE.mpu6050.connected &&
            STATE.mpl3115.connected)
        {
            if (STATE.hmc5883.bufferFilled &&
                STATE.hmc5883.calibrated &&
                //STATE.mpl3115.bufferFilled &&
                STATE.mpl3115.calibrated &&
                STATE.mpu6050.accBufferFilled &&
                STATE.mpu6050.gyroBufferFilled &&
                STATE.mpu6050.accMatrixSet &&
                STATE.mpu6050.gyroBiassed)
            {
                if (sensorLED + configTICK_RATE_HZ / 2 < taskSleep)
                {
                    LEDG_TOGGLE;
                    sensorLED = taskSleep;
                }
            }
            else
            {
                if (sensorLED + configTICK_RATE_HZ / 4 < taskSleep)
                {
                    LEDG_TOGGLE;
                    sensorLED = taskSleep;
                }
            }
        }
        else
        {
            LEDG_OFF;
            sensorLED = taskSleep;
        }

        checkFlightMode();

        logTick();
	}
}


int main(void)
{
    maintemp();

    /*{
        __OSCCONbits_t oscBits;

        asm volatile("di");
        asm volatile("ehb");

        if(!DMACONbits.SUSPEND)
		{
			DMACONSET=_DMACON_SUSPEND_MASK;		// suspend
			while((DMACONbits.DMABUSY));	// wait to be actually suspended
		}

        SYSKEY = 0, SYSKEY = 0xAA996655, SYSKEY = 0x556699AA;

        oscBits.w=OSCCON;		// read to be in sync. flush any pending write
        oscBits.PBDIV=0;
        oscBits.w|=0;
        OSCCON=oscBits.w;		// write back
        oscBits.w=OSCCON;		// make sure the write occurred before returning from this function

        asm volatile("di");
        asm volatile("ehb");

        if(!DMACONbits.SUSPEND)
		{
			DMACONSET=_DMACON_SUSPEND_MASK;		// suspend
			while((DMACONbits.DMABUSY));	// wait to be actually suspended
		}

        SYSKEY = 0, SYSKEY = 0xAA996655, SYSKEY = 0x556699AA;
    }*/

    
    //mCheConfigure(CHE_CONF_WS2 | CHE_CONF_PF_C);
    if (0)
    {
        CheKseg0CacheOn();
        CHECON = (2 << 0 ) | (1 << 4);
        INTCONSET = _INTCON_MVEC_MASK;
    }
//    INTCONSET =

	xTaskCreate(TaskMain, "Main", 170, (void*) NULL, 1, &_taskMain);
    vTaskStartScheduler();

	return 0;
}

volatile unsigned int bench_idle_time = 0;
volatile unsigned int bench_idle_time_prev;
void vApplicationIdleHook()
{
    // 1.63M wo. sensor / wo. stability
    // 1.36M w. sensor / wo. stab
    // 1.11M w. sensor / w. stab

    bench_idle_time++;
    BENCH_IDLE;
    BENCH_IDLE;
    BENCH_IDLE;
    BENCH_IDLE;
    BENCH_IDLE;
    BENCH_IDLE;
}

void vApplicationTickHook()
{
	portNOP();
    int i;
    for (i=0;i<8;i++)
    {
        //BUZZ_TOGGLE;
    }
}

// debug variables
TaskHandle_t * stack_task;
signed char * stack_taskName;

void vApplicationStackOverflowHook( TaskHandle_t * task, signed char * taskName )
{
	stack_task = task;
	stack_taskName = taskName;

    INTDisableInterrupts();

    BUZZ_ON;
    buzzError = buzzErrorLoadValue;
    while (++buzzError != 0) portNOP();
    BUZZ_OFF;

	while (1) {
		portNOP();
	}
}

void vApplicationMallocFailedHook(void)
{
    INTDisableInterrupts();

    BUZZ_ON;
    buzzError = buzzErrorLoadValue;
    while (++buzzError != 0) portNOP();
    BUZZ_OFF;
	while (1) {
		portNOP();
	}
}


static unsigned int _excep_code;
static unsigned int _excep_addr;

static enum {
	EXCEP_IRQ = 0,      // interrupt
	EXCEP_AdEL = 4,     // address error exception (load or ifetch)
	EXCEP_AdES,         // address error exception (store)
	EXCEP_IBE,          // bus error (ifetch)
	EXCEP_DBE,          // bus error (load/store)
	EXCEP_Sys,          // syscall
	EXCEP_Bp,           // breakpoint
	EXCEP_RI,           // reserved instruction
	EXCEP_CpU,          // coprocessor unusable
	EXCEP_Overflow,     // arithmetic overflow
	EXCEP_Trap,         // trap (possible divide by zero)
	EXCEP_IS1 = 16,     // implementation specfic 1
	EXCEP_CEU,          // CorExtend Unuseable
	EXCEP_C2E           // coprocessor 2
} _excep_code;

void _general_exception_handler(void)
{
    asm volatile("mfc0 %0,$13" : "=r" (_excep_code));
    asm volatile("mfc0 %0,$14" : "=r" (_excep_addr));

    _excep_code = (_excep_code & 0x0000007C) >> 2;

    INTDisableInterrupts();

    BUZZ_ON;
    buzzError = buzzErrorLoadValue;
    while (++buzzError != 0) portNOP();
    BUZZ_OFF;

    while (1)
    {
		portNOP();
    }
}

static const char * assertFile;
static unsigned long assertLine;

void vAssertCalled( const char *pcFileName, unsigned long ulLine )
{
    assertFile = pcFileName;
    assertLine = ulLine;

    while(1)
    {
        portNOP();
    }
}